#include "mex.h"

void gmc_pan(double *YCur, int row, int col, double *avgPre, double *avgCur, double *pSAD, double *pVEC, int Slices, int bFirst);
void fullsearch_1d(double *dest, int dest_w, double *scr, int src_w,double *pSAD, double *pVEC);

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	double *pYCur;
    double *pAvgPre;
    double *pAvgCur;
    double *pSAD,*pVEC;
	int     i,row,col,bFirst,Slices;
	int     a;
    
    row = mxGetM(prhs[0]); 
    col = mxGetN(prhs[0]);     
	pYCur   = mxGetPr(prhs[0]);
	pAvgPre = mxGetPr(prhs[1]);
	Slices  = (int)mxGetScalar(prhs[2]);
	bFirst  = (int)mxGetScalar(prhs[3]);
	
    plhs[0] = mxCreateDoubleMatrix(1,row+col,mxREAL);  //pAvgCur
    pAvgCur = mxGetPr(plhs[0]);
    plhs[1] = mxCreateDoubleMatrix(1,Slices*2,mxREAL);  //pSAD
    pSAD    = mxGetPr(plhs[1]);
    plhs[2] = mxCreateDoubleMatrix(1,Slices*2,mxREAL);  //pVEC
    pVEC    = mxGetPr(plhs[2]);
    
	gmc_pan(pYCur,row,col,pAvgPre,pAvgCur,pSAD,pVEC,Slices,bFirst);
	
}
    

void gmc_pan(double *YCur, int row, int col, double *pAvgPre, double *pAvgCur, double *pSAD, double *pVEC, int Slices, int bFirst)
{
  int m,n;
  int swh,swv;
  int overlap_h,overlap_v,overlap;
  int slice_len,slice_idx;
  int a;
  
  swh = 100;
  overlap_h = 0;
  swv = 100;
  overlap_v = 0;

  for(m=0;m<col;m++)
  {
     *(pAvgCur+m)= 0;  
     for (n=0;n<row;n++) 
        *(pAvgCur+m) += *(YCur+m*row+n);
     //a = (*(pAvgCur+m)+row/2)/row;
     //*(pAvgCur+m)= (double)(a);
  }
  
  for(m=0;m<row;m++)
  {
     *(pAvgCur+col+m) = 0;  
     for (n=0;n<col;n++)
        *(pAvgCur+col+m) += *(YCur+n*row+m);
     //a = (*(pAvgCur+col+m)+col/2)/col;
     //*(pAvgCur+col+m) = (double)(a);
  }

  if (bFirst==1) return;
  
  //Horizontal
 
  slice_idx = 0;
  slice_len = (col-2*swh)/Slices; 
  //printf("scr0=%d\n",pAvgCur);
  
  for (n=0;n<Slices;n++)
  {
  	overlap = (n==0) ? 0:overlap_h;
    fullsearch_1d(pAvgPre,col,(pAvgCur+n*slice_len+swh-overlap),(slice_len+overlap),(pSAD+slice_idx),(pVEC+slice_idx));
    *(pVEC+slice_idx) -= n*slice_len+swh-overlap;
    slice_idx++;
  }
  
  //printf("Vertical ==============================\n");
  //Vertical
  slice_len = (row-2*swv)/Slices;
  for (n=0;n<Slices;n++)
  {
  	overlap = (n==0) ? 0:overlap_v;
    fullsearch_1d(pAvgPre+col,row,(pAvgCur+col+n*slice_len+swv-overlap),(slice_len+overlap),pSAD+slice_idx,pVEC+slice_idx);
    *(pVEC+slice_idx) -= n*slice_len+swv-overlap;
    slice_idx++;
  }
}


void fullsearch_1d(double *dest, int dest_w, double *scr, int src_w, double *pSAD, double *pVEC)
{
  int i,k,kk;
  int delta_sum;
  int a;
  *pSAD = 1.7e+308;
  *pVEC  = 0;
  
  //a = *scr;
  //printf("dest_w=%d,src_w=%d,a=%d,scr=%d\n",dest_w,src_w,a,scr);
  
  for (i=0;i<=(dest_w-src_w);i++)
  {
  	delta_sum = 0;
  	for (k=0;k<src_w;k++)
    {  
       delta_sum += abs(*(dest+i+k)-*(scr+k));
       //a= *(dest+i); b= *(scr+k);
       //printf("delta_sum=%d,dest=%d,scr=%d\n",delta_sum,a,b);
  	}
  	//printf("%d: delta_sum=%d\n",i,delta_sum);
  	if (delta_sum < *pSAD)
  	{   
  		*pSAD = delta_sum;
  	    *pVEC = i;
  	} 
  }
}